#!/usr/bin/env python3
import rclpy
from rclpy.node import Node

from geometry_msgs.msg import Twist
from pynput import keyboard

class KeyboardCmdVel(Node):
    '''
        后台监控键盘，然后把数据发送出去
    '''
    def __init__(self):
        '''
            注册独立的发布器
        '''
        super().__init__('KeyboardCmdVel_node')
       

        self.cmd_Twist=Twist()
        self.linear_Vel=0.1
        self.angular_Vel=1.0

        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 1)
        # 创建键盘监听器
        self.listener = None  # 初始化时设置为None
        self.listener = keyboard.Listener(on_press=self.on_press, on_release=self.on_release)
        self.listener.start()
        
        self.get_logger().info("Keyboard listener started.")
    
    # def start_listening(self):
    #     # 如果监听器尚未启动，才启动监听器
    #     if self.listener is None or not self.listener.running:
    #         self.listener = keyboard.Listener(on_press=self.on_press, on_release=self.on_release)
    #         self.listener.start()
    #         self.get_logger().info("Keyboard listener started.")
    #     else:
    #         self.get_logger().info("Keyboard listener is already running.")

        

    # def stop_listening(self):
    #     self.get_logger().info("Stopping listener...")
    #     self.listener.stop()
    #     # 等待监听器停止
    #     # self.listener.join()

    def on_press(self,key):
     
        # 检查按下的键是否为上下左右 - wsad
        # try:
        #     if hasattr(key, 'char') and key.char in ['w', 's', 'a', 'd']:
        #         # 判断按键
        #         if key.char=='w':
        #             self.cmd_Twist.linear.x=self.linear_Vel
                    
        #         if key.char=='s':
        #             self.cmd_Twist.linear.x=-self.linear_Vel

        #         if key.char=='a':
        #             self.cmd_Twist.angular.z=self.angular_Vel

        #         if key.char=='d':
        #             self.cmd_Twist.angular.z=-self.angular_Vel

        #         #   发布ROS话题
        #         self.cmd_vel_pub.publish(self.cmd_Twist)
        # except AttributeError:
        # # 处理其他特殊键
        #     pass

        if key in [keyboard.Key.up, keyboard.Key.down, keyboard.Key.left, keyboard.Key.right]:
            # 判断按键
            if key==keyboard.Key.up:
                self.cmd_Twist.linear.x=self.linear_Vel
                
            if key==keyboard.Key.down:
                self.cmd_Twist.linear.x=-self.linear_Vel
                
            if key==keyboard.Key.left:
                self.cmd_Twist.angular.z=self.angular_Vel
                
            if key==keyboard.Key.right:
                self.cmd_Twist.angular.z=-self.angular_Vel
               
            #   发布ROS话题
            self.cmd_vel_pub.publish(self.cmd_Twist)


    def on_release(self,key):
        '''
            松开按键：
        '''
        # try:
        #     if hasattr(key, 'char') and key.char in ['w', 's', 'a', 'd']:
        #         '''释放速度，转换为0'''
        #         self.cmd_Twist.angular.z=0.0
        #         self.cmd_Twist.linear.x=0.0
        #         self.cmd_vel_pub.publish(self.cmd_Twist)
            
        # except AttributeError :
        #     pass
    
        if key in [keyboard.Key.up, keyboard.Key.down, keyboard.Key.left, keyboard.Key.right]:
            '''释放速度，转换为0'''
            self.cmd_Twist.angular.z=0.0
            self.cmd_Twist.linear.x=0.0
            self.cmd_vel_pub.publish(self.cmd_Twist)


def main(args=None):
    rclpy.init(args=args)
    node = KeyboardCmdVel()
    # node.start_listening()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
   
    while True:
        try:
            main()
        except KeyboardInterrupt:
            print("Shutting down")
            break
        except Exception as e:
            print("An error occurred:", str(e))
            input("Press Enter to continue...")
           